//
// Created by ysbf on 2021/2/15.
//

#include "ms5611.h"


uint32_t MS5611::cmd_adc(char cmd) {
  uint16_t ret;
  uint8_t temp[4];
  temp[0] = CMD_ADC_CONV + cmd;

  m_spi_write(temp, 1);
  switch (cmd & 0x0fu) // wait necessary conversion time. Max time for ADC used to be safe.
  {
  case CMD_ADC_256 : m_delay(ceil(600/1000.0));
    break;
  case CMD_ADC_512 : m_delay(ceil(1170/1000.0));
    break;
  case CMD_ADC_1024: m_delay(ceil(2280/1000.0));
    break;
  case CMD_ADC_2048: m_delay(ceil(4540/1000.0));
    break;
  case CMD_ADC_4096: m_delay(ceil(9040/1000.0));
    break;
  };
  temp[0] = CMD_ADC_READ;

  m_spi_write(temp, 4);
  return temp[1] * 65536 + temp[2] * 256 + temp[3];
}
uint32_t MS5611::getPressure() {
  long pressure = 0;
  pressure = cmd_adc(CMD_ADC_D1 + CMD_ADC_4096);
  return pressure;
}
double MS5611::getPressureCompensated() {

  float dT, T2;    //实际和参考温度之间的差异,实际温度,中间值
  double OFF, SENS;            //实际温度抵消,实际温度灵敏度
  double OFF2, SENS2; //温度校验值

  dT = getTemperature() - C[5] * pow(2, 8);
  uint32_t pressure = cmd_adc(CMD_ADC_D1 + CMD_ADC_4096);

  OFF = C[2] * pow(2, 16) + dT * C[4] / pow(2, 7);
  SENS = C[1] * pow(2, 15) + dT * C[3] / pow(2, 8);

  double temperature = getTemperatureCompensated();

  if (temperature < 20) {

    T2 = (dT * dT) / 0x80000000u;
    OFF2 = (float) (2.5) * (temperature - 2000) * (temperature - 2000);
    SENS2 = (float) (1.25) * (temperature - 2000) * (temperature - 2000);
    if (temperature < -1500) {
      OFF2 += 7.0 * (temperature + 1500) * (temperature + 1500);
      SENS2 += 5.5 * (temperature + 1500) * (temperature + 1500);
    }
  } else {
    T2 = 0;
    OFF2 = 0;
    SENS2 = 0;
  }

  temperature -= T2;
  OFF -= OFF2;
  SENS -= SENS2;

  double P = (((pressure * SENS) / pow(2, 21) - OFF) / pow(2, 15)) / 100;
  return P;

}
uint32_t MS5611::getTemperature() {
  uint32_t temperature = cmd_adc(CMD_ADC_D2 + CMD_ADC_4096);
  return temperature;

}
double MS5611::getTemperatureCompensated() {

  double dT = getTemperature() - C[5] * pow(2, 8);
  double T = (2000 + (dT * C[6]) / pow(2, 23)) / 100;
  return T;
}
void MS5611::cmd_reset() {
  uint8_t temp = CMD_RESET;
  m_spi_write(&temp, 1); // send reset sequence
  m_delay(3); // wait for the reset sequence timing
}
uint16_t MS5611::cmd_prom(char coef_num) {
  uint16_t rC = 0;
  uint8_t temp[3]{0};
  temp[0] = CMD_PROM_RD + coef_num * 2;
  m_spi_write(temp, 3); // send PROM READ command
  rC = temp[1] & 0x0f * 256 + temp[2];
  return rC;
}
uint8_t MS5611::crc4(uint16_t *prom) {

  int32_t i, j;
  uint32_t res = 0;
  uint8_t zero = 1;
  uint8_t crc = prom[7] & 0xF;
  prom[7] &= 0xFF00;

  // if eeprom is all zeros, we're probably fucked - BUT this will return valid CRC lol
  for (i = 0; i < 8; i++)
  {
    if (prom[i] != 0)
      zero = 0;
  }

  if (zero)
    return 0;

  for (i = 0; i < 16; i++) {
    if (i & 1)
      res ^= ((prom[i >> 1]) & 0x00FF);
    else
      res ^= (prom[i >> 1] >> 8);
    for (j = 8; j > 0; j--) {
      if (res & 0x8000)
        res ^= 0x1800;
      res <<= 1;
    }
  }

  prom[7] |= crc;
  if (crc == ((res >> 12) & 0xF))
    return 1;
  return 0;


  //  int cnt; // simple counter
  //  uint16_t n_rem; // crc reminder
  //  uint16_t crc_read; // original value of the crc
  //  unsigned char n_bit;
  //  n_rem = 0x00;
  //  crc_read = n_prom[7]; //save read CRC
  //  n_prom[7] = (0xff00u & (n_prom[7])); //CRC byte is replaced by 0
  //  for (cnt = 0; cnt < 16; cnt++) // operation is performed on bytes
  //  { // choose LSB or MSB
  //    if (cnt % 2 == 1)
  //      n_rem ^= (uint8_t) ((n_prom[cnt >> 1u]) & 0x00ffu);
  //    else
  //      n_rem ^= (uint8_t) (n_prom[cnt >> 1u] >> 8u);
  //    for (n_bit = 8; n_bit > 0; n_bit--) {
  //      if (n_rem & (0x8000u)) {
  //        n_rem = (n_rem << 1u) ^ 0x3000u;
  //      } else {
  //        n_rem = (n_rem << 1u);
  //      }
  //    }
  //  }
  //  n_rem = (0x000fu & (n_rem >> 12u)); // // final 4-bit reminder is CRC code
  //  n_prom[7] = crc_read; // restore the crc_read to its original place
  //  return (n_rem ^ 0x00);
}
void MS5611::init() {
  //  do {
  cmd_reset(); // reset the module after powerup
  m_delay(500);
  for (int i = 0; i < 8; i++) {
    C[i] = cmd_prom(i);
  } // read calibration coefficients
  n_crc = crc4(C);
  //  } while (n_crc != 1);
}
float MS5611::getEstimatedAltitude() {

  float  baroPressure=getPressureCompensated();
  float altitude = 4433000.0f * (1 - powf((((float) baroPressure) /101325), 0.190295f));

  return altitude;
}
MS5611::MS5611(MS5611::spi_read_type spi_read, MS5611::spi_write_type spi_write, MS5611::spi_async_read_type spi_async_read, MS5611::spi_async_write_type spi_async_write, MS5611::delay_type delay) : m_spi_read(spi_read), m_spi_write(spi_write), m_spi_async_read(spi_async_read), m_spi_async_write(spi_async_write), m_delay(delay){}





























